package com.fitzpatrickit.robotics.hexaped;

import java.util.Queue;

import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class SonarSampler implements Runnable {
	
	private volatile boolean run = true;
	
	private volatile boolean objDetected = false;
	
	private Thread sonarThread = new Thread(this);
	
	private UltrasonicSensor sonar = null;
	
	private int distance;
	
	private final int numTransSamples;
	
	public SonarSampler(SensorPort port, int distance, int numTransSamples) {
		this.distance = distance;
		this.numTransSamples = numTransSamples;
		
		sonar = new UltrasonicSensor(port);
		sonarThread.start();
	}
	
	public void stop() {
		run = false;
	}
	
	public boolean getObjectDetected() {
		return this.objDetected;
	}

	@Override
	public void run() {
		Queue<Boolean> detectionQueue = new Queue<Boolean>();
		
		try {
			while (run) {
				
				if (detectionQueue.size() == numTransSamples) {
					detectionQueue.pop();
				}
				
				int distance = sonar.getDistance();
				
				detectionQueue.push(distance < this.distance);
				
				boolean localObjDetected = false;
				
				if (detectionQueue.size() == numTransSamples) {
					
					if (this.objDetected) {
						localObjDetected = false;
						
						int i = 0;
						
						for (i = 0; i < detectionQueue.size(); i++) {
							if (detectionQueue.elementAt(i)) {
								localObjDetected = true;
								break;
							}
						}
						
					} else {
						localObjDetected = true;
						
						int i = 0;
						
						for (i = 0; i < detectionQueue.size(); i++) {
							if (!detectionQueue.elementAt(i)) {
								localObjDetected = false;
								break;
							}
						}
					}
				}
				
				this.objDetected = localObjDetected;
			
				Thread.yield();
			}
		} finally {
			// nothing to do
		}
	}
}
